#!/usr/bin/env python
# -*- coding: utf-8 -*-
 
import rospy
import sys
import time
import math
from geometry_msgs.msg import PoseStamped
from tf.transformations import euler_from_quaternion
from geometry_msgs.msg import Twist
import inspect

vel_msg = Twist()
vel_msg.linear.x = 0.0
vel_msg.angular.z = 0.0

car_y = 0
car_x = 0
yaw = 0

P = 5.5
D = 4.5
I = 0.1

err_angle = 0
last_err_angle = 0
last_last_err_angle = 0
def location_callback(msg):
    global car_x, car_y, yaw
    car_x = int(msg.pose.position.x *1000)
    car_y = int(msg.pose.position.y *1000)
    orientation = msg.pose.orientation
    #将四元数转换为欧拉角
    (roll, pitch, yaw) = euler_from_quaternion([orientation.x, orientation.y, orientation.z, orientation.w])
    yaw = int(math.degrees(yaw))
    # print "car_x:", car_x, "\ncar_y:", car_y, "\nyaw:", yaw
    # print("----")
    # print "robot_x:", robot_x, "\nrobot_y:", robot_y
    # print "relative_x:", relative_x, "\nrelative_y:", relative_y
    # print "relative_x * width_scale:", relative_x * width_scale, "\nwidth_scale:", width_scale
    # num_args = len(args)
    # print(num_args)
# vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
vel_pub = rospy.Publisher("/epuck_robot_0/mobile_base/cmd_vel", Twist, queue_size=10)

def computedistance(x1, y1, x2, y2):
    return math.sqrt((x2 - x1)**2 + (y2 - y1)**2)


def control_car_speed(linear_speed, angular_speed):
    
    max_x = 10
    max_a = 1
    if linear_speed > max_x:
        linear_speed = max_x
    if linear_speed < -max_x:
        linear_speed = -max_x
    if angular_speed < -max_a:
        angular_speed = -max_a
    if angular_speed > max_a:
        angular_speed = max_a

    vel_msg.linear.x = linear_speed  # 设置线速度
    vel_msg.angular.z = angular_speed  # 设置角速度
    print("vel_msg.linear.x",vel_msg.linear.x)
    print("vel_msg.angular.z",vel_msg.angular.z)
    vel_pub.publish(vel_msg)
    

class PIDController:
    def __init__(self, kp, ki, kd):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.prev_error = 0
        self.integral = 0

    def compute(self, error):
        # PID计算        
        global last_err_angle
        global last_last_err_angle
        last_last_err_angle = last_err_angle
        last_err_angle = error
        I_all = error + last_err_angle + last_last_err_angle
        D_all = error - 2 * last_err_angle + last_last_err_angle
        output = P * error - D * D_all + I * I_all
        
        # self.integral += error
        # derivative = error - self.prev_error
        # self.prev_error = error
        # output = self.kp * error + self.ki * self.integral - self.kd * derivative
        output = 0.001 * output
        max = 1
        if output > max:
            output = max
        if output < -max:
            output = -max
        return output
def get_angle_err(num):
    global yaw
    target_goal_list = [[505,389],[496,1468],[1973,1296],[1909,361]]
    err_x = target_goal_list[num][0] - car_x
    err_y = target_goal_list[num][1] - car_y
    target_angle = math.atan2(err_y, err_x)
    target_angle = math.degrees(target_angle)
    if 90 <= yaw <= 180 and -180 <= target_angle <= -160:
        target_angle = target_angle + 360
    if 160 <= target_angle <= 180 and -180 <= yaw <= -90: #
        yaw = yaw + 360
    angle_error =  target_angle - yaw
    # if abs(angle_error) > 340:
    #     angle_errortemp = 360 - abs(angle_error)
    #     angle_error = -angle_errortemp if angle_error > 0 else angle_errortemp 
    # print "target_angle", target_angle, "\nyaw", yaw, "\nangle_error", angle_error
    # print "goal_x", target_goal_list[num][0],"\ngoal_y", target_goal_list[num][1]
    return angle_error, err_x, err_y,target_angle
    
if __name__ == '__main__':
    rospy.init_node('car_control_node')  # 初始化ROS节点
    
    pid_controller = PIDController(P, I, D)
    num = 0
    # rospy.Subscriber('/vrpn_client_node/tb3_0/pose', PoseStamped, location_callback,queue_size=10)
    rospy.Subscriber('/vrpn_client_node/sppuck1/pose', PoseStamped, location_callback,queue_size=10)
    angle_error,err_x, err_y,target_angle = get_angle_err(num)
    
    while not rospy.is_shutdown(): 
        angle_error,err_x, err_y,target_angle = get_angle_err(num)
        print("angle_error", angle_error, "\nyaw", yaw, "\ntarget_angle", target_angle)
        print("num", num)
        # time.sleep(10)
        rate = rospy.Rate(10)
        while not -8 < angle_error < 8:
            angle_error,err_x, err_y,target_angle = get_angle_err(num)
            # print "target_angle", target_angle, "\nyaw", yaw, "\nangle_error", angle_error
            # print "target_x:", target_goal_list[num][0], "\ntarget_y:", target_goal_list[num][1]
            # print "location_x:", car_x, "\nlocation_y:", car_y
            angular_velocity = pid_controller.compute(angle_error)
            # print("angular_velocity",angular_velocity)
            vel_msg.linear.x = 0 
            vel_msg.angular.z = angular_velocity 
            vel_pub.publish(vel_msg)
            # print "vel_msg.linear.x", vel_msg.linear.x
            # print("angle")
            time.sleep(0.05)
            
        while not rospy.is_shutdown():
            angle_error,err_x, err_y,target_angle = get_angle_err(num)
            # print "err_x", err_x, "\nerr_y", err_y
            # print "car_x", car_x, "\ncar_y", car_y
            angular_velocity = pid_controller.compute(angle_error)
            
            
            if -50 < err_x < 50 and -50 < err_y < 50:
                vel_msg.linear.x = 0  # 设置线速度
                vel_msg.angular.z = 0
                vel_pub.publish(vel_msg)
                print("have arrive goal--------------------------------------------------------------",num)
                num += 1
                if num > 3:
                    num = 0
                break
            else:
                vel_msg.linear.x = 10
                vel_msg.angular.z = angular_velocity
                print("vel_msg.angular.z",vel_msg.angular.z)
                # print("vel_msg.angular.x",vel_msg.angular.x)
                vel_pub.publish(vel_msg)
            rate.sleep()
            
